/**
 * @file utilities.h
 * @brief header files for utility functions 
 * @author Wayne Zhang                         
*/

#ifndef UTILS_H
#define UTILS_H

/** zero an array */
#define AZERO(x, n) {int _I_, _SZ_ = (n); for(_I_ = 0; _I_ < _SZ_; _I_++) (x)[_I_] = 0;}


/* inline functions */

/**
 * cumulative sum of a vector of double 
 *
 * @param x double vector to be summed
 * @param n number of elements
 *
 * @return cumulative sum
 */
static R_INLINE double dcumsum(double *x, int n){
  double sm = 0.0 ;
  for (int i = 0; i < n; i++)  sm += x[i] ;
  return sm ;
}

/**
 * inner product of two double vectors
 *
 * @param x double vector
 * @param y double vector
 * @param n number of elements
 *
 * @return inner product
 */
static R_INLINE double dinner(double *x, double *y, int n){
  double sm = 0.0 ;
  for (int i = 0; i < n; i++)  sm += x[i] * y[i];
  return sm ;
}


/**
 * get the maximum value of a double vector
 *
 * @param x a double vector
 * @param n length of the vector
 *
 * @return the maximum value
 */
static R_INLINE double dmax (double *x, int n){
  double s = x[0] ;
  for (int i = 1; i < n; i++)
      if (x[i] > s) s = x[i] ; 
  return s ;
}

/**
 * get the minmimum value of a double vector
 *
 * @param x a double vector
 * @param n length of the vector
 *
 * @return the minimum value
 */
static R_INLINE double dmin(double *x, int n){
    double s = x[0];
    for (int i = 1; i < n; i++){
	if (x[i] < s) s = x[i];
    }
    return s;
}

/**
 * get the maximum value of an int vector
 *
 * @param x an int vector
 * @param n length of the vector
 *
 * @return the maximum value
 */
static R_INLINE int imax (int *x, int n){
  int s = x[0] ;
  for (int i = 1; i < n; i++)
      if (x[i] > s) s = x[i] ; 
  return s ;
}


/**
 * Compute sample mean
 *
 * @param n number of samples
 * @param x samples in long vector 
 *
 * @return mean 
 */
static R_INLINE double mean(double *x, int n){
  return dcumsum(x, n) / n ;
}


/**
 * Return the sum of squares of the first n elements of x
 *
 * @param n
 * @param x
 *
 * @return sum of squares
 */
static R_INLINE double sqr_length(double *x, int n)
{
    double ans = 0;
    for (int i = 0; i < n; i++) ans += x[i] * x[i];
    return ans;
}


/**
 * compute the (relative) distance between two vectors 
 *
 * @param x a numeric vector
 * @param y a numeric vector used to normalize the distance
 * @param n length of the vectors
 * @param rel a binary indicator of whether normalization is performed
 *
 * @return the (relative) distance
 */
static R_INLINE double dist(double *x, double *y, int n, int rel){
  double dxy = 0, dy;
  for (int i = 0; i < n; i++)
    dxy += (x[i] - y[i]) * (x[i] - y[i]);
  if (rel){
    dy = sqr_length(y, n);
    return(sqrt(dxy)/ (1 + sqrt(dy)));
  } else 
    return(sqrt(dxy));

}



/**
 * return the index at which the max occurs
 * (if there is a tie, returns the first)
 * 
 * @param x the input vector
 * @param n the length of the input vector 
 * 
 * @return the index 
 */
static R_INLINE int which_max(double *x, int n){
  double d = x[0] ;
  for (int i = 1; i < n; i++)
      if (x[i] > d) d = x[i];   
  for (int i = 0; i < n; i++)
    if (x[i] == d)  return(i);
  return(0);
}


/**
 * count the number of nonzero elements in a vector
 * 
 * @param x the input vector
 * @param n the length of the input vector 
 * 
 * @return the number of nonzero elements 
 */

static R_INLINE int nnzero(double *x, int n){
  int nnz = 0;
  for (int i = 0; i < n; i++)
    if (x[i] != 0) nnz++;
  return(nnz);
}


/** 
 *  Soft-thresholding rules
 *
 * @param x value to be thresheld 
 * @param l the threshold
 * 
 */
static R_INLINE double TH_soft(double x, double l){
  if (x > l) return(x - l);
  if (x < -l) return(x + l);
  return(0);
}


/** 
 *  Hard-thresholding rules
 *
 * @param x value to be thresheld 
 * @param l the threshold
 * 
 */
static R_INLINE double TH_hard(double x, double l){
  if (x > l || x < -l) return(x);
  return(0);
}


/** 
 *  Group Lasso thresholding rules (1 - lam/||x||)_+ * x
 *
 * @param x value to be thresheld 
 * @param n length of x
 * @param l the threshold
 * 
 * @return the norm of the updated x
 */
static R_INLINE double TH_glasso(double *x, int n, double l){
  double zn = sqrt(sqr_length(x, n));
  double z = 1 - l/zn; 
  if (z <= 0){ 
    for (int i = 0; i < n; i++) 
      x[i] = 0.0;
  } else{
    for (int i = 0; i < n; i++) 
      x[i] *= z;
  }
  zn = sqrt(sqr_length(x, n));     // norm of the new x
  return(zn);
}

/**
 * Permute the vector src according to perm into dest
 *
 * @param dest destination
 * @param src source
 * @param perm NULL or 0-based permutation of length n
 * @param n length of src, dest and perm
 *
 * @return dest
 *
 * \note If perm is NULL the first n elements of src are copied to dest.
 */
static R_INLINE double*
apply_perm(double *dest, const double *src, const int *perm, int n)
{
    for (int i = 0; i < n; i++) dest[i] = src[perm ? perm[i] : i];
    return dest;
}



/* prototypes defined in utilities.c */


void cov(int n, int p, double *x, double *ans);

// matrix computation
void sum_mat(int m, int n, int margin, double *x, double *ans);
void mult_xtx(int m, int n, double *x, double *out);
void mult_mv(char *trans, int m, int n, double *A,
             double *x, double *out);
void chol(int d, double *v, double *iv);
int solve_po(int d, double *v, double *iv);
int solve_po2(int d, double *A, double *b, double *x);

// numerical derivatives
void grad(int n, double *x, 
          double (*myfunc)(double *x, void *data), 
          void *data, double *ans) ;
void hess(int n, double *x, 
          double (*myfunc)(double *x, void *data), 
          void *data, double *ans) ;

// wishart simulation
void rwishart(int d, double nu, double *scal, double *out);


#endif
